/**
 * @file
 * @brief Implementation of CanbusHandler
 */

#include "canbus/handler.h"

namespace canbus {

CanbusHandler::CanbusHandler(std::string id, int channel, long can_rate, uint8_t can_msg_type):
PHYLayerHandler(),
id_(id),
channel_id_(channel),
can_rate_(can_rate),
can_msg_type_(can_msg_type)
{}

CanbusHandler::~CanbusHandler() {
  Stop();
}

bool CanbusHandler::Init() {
  bool flags = can_msg_type_ == canMSG_STD?0:canOPEN_REQUIRE_EXTENDED;
  tx_ = canOpenChannel(channel_id_, flags);
  ROS_WARN_STREAM_COND(tx_ == canINVALID_HANDLE, 
    FNAME << "canOpenChannel not OK, error code: " << tx_);

  rx_ = canOpenChannel(channel_id_, flags);
  ROS_WARN_STREAM_COND(rx_ == canINVALID_HANDLE, 
    FNAME << "canOpenChannel not OK, error code: " << rx_);

  if (tx_ == canINVALID_HANDLE || rx_ == canINVALID_HANDLE) {
    return false;
  } 
  return true;
}

bool CanbusHandler::Configure() {
  canStatus tx_bus_param = canSetBusParams(rx_, can_rate_, 0, 0, 0, 0, 0);
  ROS_WARN_STREAM_COND(CANSTATUS_FAILURE(tx_bus_param), 
    FNAME << "canSetBusParams not OK, error code: " << tx_bus_param);

  canStatus tx_bus_on = canBusOn(rx_);
  ROS_WARN_STREAM_COND(CANSTATUS_FAILURE(tx_bus_on), 
    FNAME << "canBusOn not OK, error code: " << tx_bus_on);

  canStatus rx_bus_param = canSetBusParams(tx_, can_rate_, 0, 0, 0, 0, 0);
  ROS_WARN_STREAM_COND(CANSTATUS_FAILURE(rx_bus_param), 
    FNAME << "canSetBusParams not OK, error code: " << rx_bus_param);

  canStatus rx_bus_on = canBusOn(tx_);
  ROS_WARN_STREAM_COND(CANSTATUS_FAILURE(rx_bus_on), 
    FNAME << "canBusOn not OK, error code: " << rx_bus_on);

  if (!tx_bus_param || !rx_bus_param || !tx_bus_on || !rx_bus_on) {
    return false;
  }
  return true;
}

void CanbusHandler::ResetMessages() {
  for(auto& p:can_msg_map_) {
    for(int i=0;i<8;i++) {
      p.second->data_[i] = 0;
    }
  }
}

MsgBase* CanbusHandler::GetDataById(const uint32_t id) {
if (can_msg_map_.find(id) == can_msg_map_.end()) {
    std::cout << FNAME << "Cannot find specified key: " << id << " from map" << std::endl;
    return nullptr;
  }
  return can_msg_map_[id];
}

bool CanbusHandler::Write(const MsgBase* data) {
  for(int i=0;i<8;i++) {send_buf.msg[i] = *(data->data_+i);}
  send_buf.id = data->id_;
  bool ret = CanWrite();
  return ret;
}

bool CanbusHandler::Read(MsgBase* data) {
  bool ret = CanRead();
  for(int i=0;i<8;i++) {*(data->data_+i) = recv_buf.msg[i];}
  return ret;
}

bool CanbusHandler::CanWrite() {
  unsigned char msg_send[8];
  for(int i=0;i<8;i++) {
    msg_send[i] = send_buf.msg[i];
    std::cout << "message ID: " << (int)send_buf.id << ", message content: " << (int)msg_send[i] << std::endl;
  }
  std::cout << "----------------------------------" << std::endl;
  auto stat = canWrite(tx_, send_buf.id, msg_send, send_buf.dlc, can_msg_type_);
  return stat;
}

bool CanbusHandler::CanRead() {
  unsigned char msg_recv[8];
  auto stat = canRead(rx_, &recv_buf.id, msg_recv, &recv_buf.dlc, &recv_buf.flag, &recv_buf.time);
  for(int i=0;i<8;i++) {recv_buf.msg[i] = msg_recv[i];}
  return stat;
}

bool CanbusHandler::Start() {
  rx_thread_.reset(new std::thread([this] { RecvThreadFunc(); }));
  ROS_WARN_STREAM_COND(rx_thread_ == nullptr, FNAME << "Recv thread failed");
  tx_thread_.reset(new std::thread([this] { SendThreadFunc(); }));
  ROS_WARN_STREAM_COND(tx_thread_ == nullptr, FNAME << "Send thread failed");
  if (rx_thread_ == nullptr || tx_thread_ == nullptr) {
    return false;
  }
  is_running_ = true;
  ROS_INFO_STREAM(FNAME << "Threads started");
  return true;
}

bool CanbusHandler::Stop() {
  if (IsRunning()) {
    is_running_ = false;
    if (rx_thread_ != nullptr && rx_thread_->joinable()) {
      rx_thread_->join();
    }
    if (tx_thread_ != nullptr && tx_thread_->joinable()) {
      tx_thread_->join();
    }
    rx_thread_.reset(nullptr);
    tx_thread_.reset(nullptr);
    ROS_INFO_STREAM(FNAME << "Threads stopped successfully");
    return true;
  } else {
    ROS_WARN_STREAM(FNAME << "Failed to stop threads, some threads may still running");
    return false;
  }
}

void CanbusHandler::SendThreadFunc() {
  bool ret;
  int n = 0;
  while (ros::ok()) {
    // ROS_INFO_STREAM(FNAME << "Channel: "<< id_ << ", looping: " << n++);
    // ret = CanWrite();
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }   
}

void CanbusHandler::RecvThreadFunc() {
  int n = 0;
  while (ros::ok()) {
    // ROS_INFO_STREAM(FNAME << "Channel: "<< id_ << ", looping: " << n++);
    if (!CanRead()) {
      GetDataById(recv_buf.id)->UpUpdate(recv_buf.msg);
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }
} 
   
} // namespace 